/*
 * Omnidrive.cpp
 *
 *  Created on: Oct 31, 2010
 *      Author: eweber
 */

#include <Omnidrive.h>

#include "GenericHID.h"
#include "Joystick.h"
#include "Jaguar.h"
#include "Utility.h"
#include "WPIStatus.h"
#include <math.h>

Omnidrive::Omnidrive(SpeedController *FL, SpeedController *FR, SpeedController *RL, SpeedController *RR){
	if (FL == NULL || FR == NULL || RL == NULL || RR == NULL){
		wpi_fatal(NullParameter);
		m_frontLeftMotor = m_rearLeftMotor = m_frontRightMotor = m_rearRightMotor = NULL;
		return;
	}

	mFrontLeft = FL;
	mFrontRight = FR;
	mRearLeft = RL;
	mRearRight = RR;
}

Omnidrive::Omnidrive(unsigned char FL, unsigned char FR, unsigned char RL, unsigned char RR){
	mFrontLeft = new Jaguar(FL);
	mFrontRight = new Jaguar(FR);
	mRearLeft = new Jaguar(RL);
	mRearRight = new Jaguar(RR);
}

Omnidrive::Omni(){
	mFrontLeft = new Jaguar(1);
	mFrontRight = new Jaguar(2);
	mRearLeft = new Jaguar(3);
	mRearRight = new Jaguar(4);
}

Omnidrive::~Omnidrive(){
	delete mFrontLeft;
	delete mFrontRight;
	delete mRearLeft;
	delete mRearRight;
}

void Omnidrive::SetPower(float in){
	mPower = in;
}

float Omnidrive::GetPower(void){
	return mPower;
}

void Omnidrive::SetHeading(float in){
	mHeading = in;
}

float Omnidrive::GetHeading(void){
	return mHeading;
}

void Omnidrive::SetFacing(float in){
	mFacing = in;
}

float Omnidrive::GetFacing(void){
	return mFacing;
}

void Omnidrive::CalculateMotorPowersAuton(void){
	mSpeedFL = mPower * sinf(mHeading) + mPower * cosf(mHeading);
	mSpeedFR = mPower * sinf(mHeading) - mPower * cosf(mHeading);
	mSpeedRL = mPower * sinf(mHeading) - mPower * cosf(mHeading);
	mSpeedRR = mPower * sinf(mHeading) + mPower * cosf(mHeading);
}

void Omnidrive::CalculateMotorPowersTeleop(void){


	mSpeedFL = mPower * sinf(mHeading) + mPower * cosf(mHeading);
	mSpeedFR = mPower * sinf(mHeading) - mPower * cosf(mHeading);
	mSpeedRL = mPower * sinf(mHeading) - mPower * cosf(mHeading);
	mSpeedRR = mPower * sinf(mHeading) + mPower * cosf(mHeading);
}

void Omnidrive::UpdateMotorsAuton(){
	CalcMotorPowersAuton();
	mFrontLeft->Set(mSpeedFL);
	mFrontRight->Set(mSpeedFR);
	mRearLeft->Set(mSpeedRL);
	mRearRight->Set(mSpeedRR);
}

void Omnidrive::UpdateMotorsTeleop(){
	CalcMotorPowersTeleop();
	mFrontLeft->Set(mSpeedFL);
	mFrontRight->Set(mSpeedFR);
	mRearLeft->Set(mSpeedRL);
	mRearRight->Set(mSpeedRR);
}
